package Strategy;

import Factory.AgriculturalMachine;
import Observer.EnvironmentData;
import java.util.ArrayList;
import java.util.List;

// 具体策略 - 避障路径规划
public class ObstacleAvoidancePathPlanning implements PathPlanningStrategy {
    private EnvironmentData data;
    private List<AgriculturalMachine> machines; // 新增：农机列表

    // 修改构造函数，接收农机列表
    public ObstacleAvoidancePathPlanning(EnvironmentData data, List<AgriculturalMachine> machines) {
        this.data = data;
        this.machines = machines;
    }

    @Override
    public Path calculatePath(FieldMap map, EnvironmentData data) {
        // 检查是否有农机配置
        if (machines == null || machines.isEmpty()) {
            System.out.println("错误: 没有配置农机");
            return new Path(); // 返回空路径
        }

        System.out.println("Calculating obstacle avoidance path for " + machines.size() + " machines...");

        // 为第一台农机创建路径对象
        Path path = new Path();

        // 设置起点和终点
        Coordinate start = new Coordinate(0, 0);
        Coordinate end = new Coordinate(map.getFieldBoundaryMax().getX(), map.getFieldBoundaryMax().getY());

        // 添加起点
        path.addWaypoint(start);

        // 获取障碍物列表
        List<Coordinate> obstacles = map.getObstaclePositions();

        // 如果没有障碍物，直接使用标准路径
        if (obstacles == null || obstacles.isEmpty()) {
            System.out.println("没有检测到障碍物，使用标准路径");
            StandardPathPlanning standardStrategy = new StandardPathPlanning(data);
            return standardStrategy.calculatePath(map, data);
        }

        // 生成避障路径
        List<Coordinate> waypoints = generateObstacleAvoidancePath(map, start, end, obstacles);

        // 添加所有路径点
        for (Coordinate waypoint : waypoints) {
            path.addWaypoint(waypoint);
        }

        // 添加终点
        path.addWaypoint(end);

        // 设置路径类型和计算路径信息
        path.setPathType("避障路径");
        path.calculateTotalDistance();
        path.calculateEstimatedTime(4); // 避障路径速度稍慢，假设为4m/s

        return path;
    }

    private List<Coordinate> generateObstacleAvoidancePath(FieldMap map, Coordinate start, Coordinate end, List<Coordinate> obstacles) {
        List<Coordinate> waypoints = new ArrayList<>();

        // 使用简单的A*算法思想进行避障路径规划
        Coordinate current = start;
        double avoidanceDistance = 3.0; // 避障距离，单位：米

        while (!isNear(current, end, 1.0)) {
            // 寻找下一个路径点
            Coordinate nextPoint = findNextPoint(map, current, end, obstacles, avoidanceDistance);

            if (nextPoint == null) {
                System.out.println("无法找到避开障碍物的路径，使用标准路径");
                StandardPathPlanning standardStrategy = new StandardPathPlanning(data);
                Path standardPath = standardStrategy.calculatePath(map, data);
                return standardPath.getWaypoints().subList(1, standardPath.getWaypoints().size() - 1);
            }

            waypoints.add(nextPoint);
            current = nextPoint;
        }

        return waypoints;
    }

    private Coordinate findNextPoint(FieldMap map, Coordinate current, Coordinate target, List<Coordinate> obstacles, double avoidanceDistance) {
        // 定义可能的方向（8个方向）
        double[][] directions = {
                {1, 0}, {1, 1}, {0, 1}, {-1, 1},
                {-1, 0}, {-1, -1}, {0, -1}, {1, -1}
        };

        Coordinate bestPoint = null;
        double bestScore = Double.NEGATIVE_INFINITY;

        // 检查每个方向
        for (double[] dir : directions) {
            // 计算新点
            Coordinate newPoint = new Coordinate(
                    current.getX() + dir[0] * 2.0,
                    current.getY() + dir[1] * 2.0
            );

            // 检查是否在边界内
            if (!map.isWithinBoundary(newPoint)) {
                continue;
            }

            // 检查是否靠近障碍物
            boolean tooClose = false;
            for (Coordinate obstacle : obstacles) {
                if (newPoint.distanceTo(obstacle) < avoidanceDistance) {
                    tooClose = true;
                    break;
                }
            }

            if (tooClose) {
                continue;
            }

            // 计算评分（接近目标 + 远离障碍物）
            double score = -newPoint.distanceTo(target);

            // 计算与所有障碍物的距离和
            for (Coordinate obstacle : obstacles) {
                score += newPoint.distanceTo(obstacle) * 0.2; // 权重因子
            }

            // 如果评分更高，更新最佳点
            if (score > bestScore) {
                bestScore = score;
                bestPoint = newPoint;
            }
        }

        return bestPoint;
    }

    private boolean isNear(Coordinate point1, Coordinate point2, double threshold) {
        return point1.distanceTo(point2) < threshold;
    }
}